#include <cmath>
#include <string>
#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float64.h>
#include <jaguar/ares_drive.h>
#include <jaguar/JaguarConfig.h>
#include <drive_commands/drive_raw.h>

using namespace can;
using namespace jaguar;

static ros::Subscriber sub_twist;
//static ros::Publisher pub_odom;
//static ros::Publisher pub_estop;
static ros::Publisher pub_temp_1, pub_temp_2, pub_temp_3, pub_temp_4;
static ros::Publisher pub_voltage_1, pub_voltage_2, pub_voltage_3, pub_voltage_4;
static ros::Publisher pub_v1, pub_v2, pub_v3, pub_v4, pub_wheel;

static ros::Time last_time;
static AresDriveSettings settings;
static boost::shared_ptr<AresDriveRobot> robot;
static boost::shared_ptr<tf::TransformBroadcaster> pub_tf;
static std::string frame_parent;
static std::string frame_child;
static int heartbeat_rate;
static double wheel_separation, wheel_diameter, alpha, gain_p, gain_i, gain_d;
static volatile bool spinlock = false;

static void callback_odom(double x, double y, double theta,
                          double velocity, double omega,
                          double v_1, double v_2, double v_3, double v_4)
{
    ros::Time now = ros::Time::now();

    // odom TF Frame
    geometry_msgs::TransformStamped msg_tf;
    msg_tf.header.stamp = now;
    msg_tf.header.frame_id = frame_parent;
    msg_tf.child_frame_id  = frame_child;
    msg_tf.transform.translation.x = x;
    msg_tf.transform.translation.y = y;
    msg_tf.transform.translation.z = 0;
    msg_tf.transform.rotation = tf::createQuaternionMsgFromYaw(theta);
    pub_tf->sendTransform(msg_tf);

    // Publish instantaneous velocity.
    std_msgs::Float64 msg_v1, msg_v2, msg_v3, msg_v4;
    msg_v1.data = v_1;
    msg_v2.data = v_2;
    msg_v3.data = v_3;
    msg_v4.data = v_4;
    pub_v1.publish(msg_v1);
    pub_v2.publish(msg_v2);
    pub_v3.publish(msg_v3);
    pub_v4.publish(msg_v4);

    // Odometry Message
   /* nav_msgs::Odometry msg_odom;
    msg_odom.header.stamp = now;
    msg_odom.header.frame_id = frame_parent;
    msg_odom.child_frame_id  = frame_child;
    msg_odom.pose.pose.position.x = x;
    msg_odom.pose.pose.position.y = y;
    msg_odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(theta);
    msg_odom.twist.twist.linear.x = velocity;
    msg_odom.twist.twist.linear.y = 0;
    msg_odom.twist.twist.angular.z = omega;
    pub_odom.publish(msg_odom);

    // TODO: Why are these flipped?
    robot_kf::WheelOdometry4 msg_wheel;
    msg_wheel.header.stamp = now;
    msg_wheel.header.frame_id = frame_child;
    msg_wheel.timestep = now - last_time;
    msg_wheel.separation = wheel_separation;
    msg_wheel.one.movement = delta_1;
    msg_wheel.one.variance = alpha * fabs(delta_1);
    msg_wheel.two.movement = delta_2;
    msg_wheel.two.variance = alpha * fabs(delta_2);
    msg_wheel.three.movement = delta_3;
    msg_wheel.three.variance = alpha * fabs(delta_3);
    msg_wheel.four.movement = delta_4;
    msg_wheel.four.variance = alpha * fabs(delta_4);
    pub_wheel.publish(msg_wheel);
*/

    last_time = now;
}

/*static void callback_estop(bool stopped)
{
    std_msgs::Bool msg;
    msg.data = stopped;
    pub_estop.publish(msg);
}*/

static void callback_diag_one(double voltage, double temperature)
{
    std_msgs::Float64 msg_voltage, msg_temperature;
    msg_voltage.data = voltage;
    msg_temperature.data = temperature;
    pub_voltage_1.publish(msg_voltage);
    pub_temp_1.publish(msg_temperature);
}

static void callback_diag_two(double voltage, double temperature)
{
    std_msgs::Float64 msg_voltage, msg_temperature;
    msg_voltage.data = voltage;
    msg_temperature.data = temperature;
    pub_voltage_2.publish(msg_voltage);
    pub_temp_2.publish(msg_temperature);
}

static void callback_diag_three(double voltage, double temperature)
{
    std_msgs::Float64 msg_voltage, msg_temperature;
    msg_voltage.data = voltage;
    msg_temperature.data = temperature;
    pub_voltage_3.publish(msg_voltage);
    pub_temp_3.publish(msg_temperature);
}

static void callback_diag_four(double voltage, double temperature)
{
    std_msgs::Float64 msg_voltage, msg_temperature;
    msg_voltage.data = voltage;
    msg_temperature.data = temperature;
    pub_voltage_4.publish(msg_voltage);
    pub_temp_4.publish(msg_temperature);
}

void callback_speed(AresDriveRobot::Wheel wheel, double speed)
{
    std_msgs::Float64 msg;
    msg.data = speed;
    switch (wheel) {
    case AresDriveRobot::kOne:
        pub_v1.publish(msg);
        break;

    case AresDriveRobot::kTwo:
        pub_v2.publish(msg);
        break;
    
    case AresDriveRobot::kThree:
        pub_v3.publish(msg);
        break;
    
    case AresDriveRobot::kFour:
        pub_v4.publish(msg);
        break;
    
    default:
        ROS_WARN_THROTTLE(10, "Invalid speed callback.");
    }
}

void callback_cmd(drive_commands::drive_raw const &drive_raw)
{
    
    ROS_INFO("1: %f, 2: %f, 3: %f, 4: %f", drive_raw.spd1, drive_raw.spd2, drive_raw.spd3, drive_raw.spd4);
    robot->drive_raw(drive_raw.spd1, drive_raw.spd2, drive_raw.spd3, drive_raw.spd4);
}

void callback_reconfigure(jaguar::JaguarConfig &config, uint32_t level)
{
    // Speed Control Gains
    if (level & 1) {
        robot->speed_set_p(config.gain_p);
        ROS_INFO("Reconfigure, P = %f", config.gain_p);
    }
    if (level & 2) {
        robot->speed_set_i(config.gain_i);
        ROS_INFO("Reconfigure, I = %f", config.gain_i);
    }
    if (level & 4) {
        robot->speed_set_d(config.gain_d);
        ROS_INFO("Reconfigure, D = %f", config.gain_d);
    }
    if (level & 8) {
        robot->drive_brake(config.brake);
        ROS_INFO("Reconfigure, Braking = %d", config.brake);
    }
    if (level & 16) {
        if (0 < config.cpr && config.cpr <= std::numeric_limits<uint16_t>::max()) {
            robot->odom_set_encoders(config.cpr);
            ROS_INFO("Reconfigure, CPR = %d", config.cpr);
        } else {
            ROS_WARN("CPR must be a positive 16-bit unsigned integer.");
        }
    }
    if (level & 32) {
        if (config.wheel_diameter <= 0) {
            ROS_WARN("Wheel diameter must be positive.");
        } else {
            robot->odom_set_circumference(M_PI * config.wheel_diameter);
            ROS_INFO("Reconfigure, Wheel Diameter = %f m", config.wheel_diameter);
        }
    }
    if (level & 64) {
        if (config.wheel_separation <= 0) {
            ROS_WARN("Wheel separation must be positive.");
        } else {
            robot->odom_set_separation(config.wheel_separation);
            wheel_separation = config.wheel_separation;
            ROS_INFO("Reconfigure, Wheel Separation = %f m", config.wheel_separation);
        }
    }
    if (level & 128) {
        if (config.odom_rate <= 0 || config.odom_rate > 255) {
            ROS_WARN("Odometry update rate must be positive.");
        } else {
            robot->odom_set_rate(config.odom_rate);
            ROS_INFO("Reconfigure, Odometry Update Rate = %d ms", config.odom_rate);
        }
    }
    if (level & 256) {
        if (config.diag_rate <= 0 || config.diag_rate > 255) {
            ROS_WARN("Diagnostics update rate must be positive.");
        } else {
            robot->diag_set_rate(config.diag_rate);
            ROS_INFO("Reconfigure, Diagnostics Update Rate = %d ms", config.diag_rate);
        }
    }
    if (level & 512) {
        if (config.heartbeat_rate <= 0 || config.heartbeat_rate > 255) {
            ROS_WARN("Heartbeat rate must be in the range 1-255.");
        } else if (config.heartbeat_rate > 100) {
            ROS_WARN("Heartbeat rate is dangerously high.");
        } else {
            heartbeat_rate = config.heartbeat_rate;
            ROS_INFO("Reconfigure, Heartbeat Rate = %d ms", config.heartbeat_rate);
        }
    }
    if (level & 1024) {
        robot->drive_raw(config.setpoint, config.setpoint, config.setpoint, config.setpoint);
        ROS_INFO("Reconfigure, Setpoint = %f", config.setpoint);
    }
    if (level & 2048) {
        if (config.alpha < 0.0) {
            ROS_WARN("Alpha must be positive");
        } else {
            alpha = config.alpha;
            ROS_INFO("Reconfigure, alpha = %f", alpha);
        }
    }
    spinlock = true;
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "ares_drive_node");
    ros::NodeHandle nh;
    
    ROS_INFO("Begin parameter acquisition");
    ros::param::get("~port", settings.port);
    ros::param::get("~id_1", settings.id_1);
    ros::param::get("~id_2", settings.id_2);
    ros::param::get("~id_3", settings.id_3);
    ros::param::get("~id_4", settings.id_4);
    ros::param::get("~frame_parent", frame_parent);
    ros::param::get("~frame_child", frame_child);
    ros::param::get("~accel_max", settings.accel_max_mps2);
    ros::param::get("~flip_1", settings.flip_1);
    ros::param::get("~flip_2", settings.flip_2);
    ros::param::get("~flip_3", settings.flip_3);
    ros::param::get("~flip_4", settings.flip_4);
    ros::param::get("~wheel_diameter", wheel_diameter);
    ros::param::get("~wheel_separation", wheel_separation);
    ros::param::get("~gain_p", gain_p);
    ros::param::get("~gain_i", gain_i);
    ros::param::get("~gain_d", gain_d);
    ROS_INFO("Parameters got.");

    // TODO: Read this from a parameter.
    last_time = ros::Time::now();
    settings.brake = BrakeCoastSetting::kOverrideCoast;


    if (!(1 <= settings.id_1 && settings.id_1 <= 63)
     || !(1 <= settings.id_2 && settings.id_2 <= 63)
     || !(1 <= settings.id_3 && settings.id_3 <= 63)
     || !(1 <= settings.id_4 && settings.id_4 <= 63)) {
        ROS_FATAL("Invalid CAN device id. Must be in the range 1-63.");
        return 1;
    } else if ((settings.id_1 == settings.id_2) || (settings.id_1 == settings.id_3)
      || (settings.id_1 == settings.id_4) || (settings.id_2 == settings.id_3)
      || (settings.id_2 == settings.id_4) || (settings.id_3 == settings.id_4)){
        ROS_FATAL("Invalid CAN device ID. Left and right IDs must be unique.");
        return 1;
    }
    ROS_INFO("Communicating to IDs %i, %i, %i, and %i over %i", settings.id_1,
             settings.id_2, settings.id_3, settings.id_4, settings.port.c_str());
    ROS_INFO("Parameter checks complete");

    robot = boost::make_shared<AresDriveRobot>(settings);
    
    robot->odom_set_circumference(3.1415936 * wheel_diameter);
    robot->odom_set_separation(wheel_separation);
    robot->speed_set_p(gain_p);
    robot->speed_set_i(gain_i);
    robot->speed_set_d(gain_d);
    //can::JaguarBridge bridge(settings.port);
    //ROS_INFO("bridge made");
    //jaguar::JaguarBroadcaster broadcaster(bridge);
    //ROS_INFO("broadcaster made");
    //jaguar::Jaguar jag1(bridge, settings.id_1), jag2(bridge, settings.id_2), jag3(bridge, settings.id_3), jag4(bridge, settings.id_4);
    //ROS_INFO("jag instances created");
    //broadcaster.system_resume();
    //ROS_INFO("Broadcaster resumed");

    // Use dynamic reconfigure for all remaining parameters.
    //dynamic_reconfigure::Server<jaguar::JaguarConfig> server;
    //dynamic_reconfigure::Server<jaguar::JaguarConfig>::CallbackType f;
    //f = boost::bind(&callback_reconfigure, _1, _2);
    //server.setCallback(f);

    sub_twist = nh.subscribe("cmd_vel_raw", 1, &callback_cmd);
    //pub_odom  = nh.advertise<nav_msgs::Odometry>("odom", 100);
    //pub_estop = nh.advertise<std_msgs::Bool>("estop", 1, true);
    pub_v1  = nh.advertise<std_msgs::Float64>("encoder_one", 10);
    pub_v2 = nh.advertise<std_msgs::Float64>("encoder_two", 10);
    pub_v3  = nh.advertise<std_msgs::Float64>("encoder_three", 10);
    pub_v4 = nh.advertise<std_msgs::Float64>("encoder_four", 10);
    //pub_wheel  = nh.advertise<robot_kf::WheelOdometry>("wheel_odom", 10);
    /*pub_temp_1  = nh.advertise<std_msgs::Float64>("temperature_one", 10);
    pub_temp_2 = nh.advertise<std_msgs::Float64>("temperature_two", 10);
    pub_temp_3  = nh.advertise<std_msgs::Float64>("temperature_three", 10);
    pub_temp_4 = nh.advertise<std_msgs::Float64>("temperature_four", 10);
    pub_voltage_1  = nh.advertise<std_msgs::Float64>("voltage_one", 10);
    pub_voltage_2 = nh.advertise<std_msgs::Float64>("voltage_two", 10);
    pub_voltage_3  = nh.advertise<std_msgs::Float64>("voltage_three", 10);
    pub_voltage_4 = nh.advertise<std_msgs::Float64>("voltage_four", 10);*/
    pub_tf = boost::make_shared<tf::TransformBroadcaster>();

    // These must be registered after the publishers are initialized. Otherwise
    // there is a race condition in the callbacks.
    //robot->odom_attach(&callback_odom);
    //robot->diag_attach(&callback_diag_one, &callback_diag_two, &callback_diag_three, &callback_diag_four);
    //robot->estop_attach(&callback_estop);

    //while (!spinlock);

    //set brakes - diff_drive lists this as needing to happen after a new boot
    /*jag1.config_brake_set(settings.brake)->block();
    jag2.config_brake_set(settings.brake)->block();
    jag3.config_brake_set(settings.brake)->block();
    jag4.config_brake_set(settings.brake)->block();
    ROS_INFO("Brakes Set");

    jag1.voltage_enable()->block();
    jag2.voltage_enable()->block();
    jag3.voltage_enable()->block();
    jag4.voltage_enable()->block();
    ROS_INFO("Voltage Control Enabled");
    */    
    // TODO: Read this heartbeat rate from a parameter.

    ros::Rate heartbeat_rate(50);
    
    int cnter = 0;
    while (ros::ok()) {
        /*jag1.voltage_set(0.7)->block();
        jag2.voltage_set(0.7)->block();
	jag3.voltage_set(0.7)->block();
	jag4.voltage_set(0.7)->block();
        ROS_INFO("Voltage Set");
        broadcaster.heartbeat();
        ROS_INFO("Heartbeat Sent");*/
        robot->drive_spin(1 / 50.);
	    robot->heartbeat();
        ros::spinOnce();
        //ROS_INFO("Spuuuun %i times",cnter);
        cnter++;
        heartbeat_rate.sleep();
    }
    return 0;
}
